﻿#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <mtAffine.h>

extern "C" {

    /*
     *      | m0 m1 m2 |
     *  M = | m3 m4 m5 |
     *      |  0  0  1 |
     */
    const static size_t     MT_AFFINE_MATRIX_2D_SZ  = 6;

    /*
     * 逆仿射矩阵
     *
     *      | m0 m1 m2 |
     *  M = | m3 m4 m5 |
     *      |  0  0  1 |
     *
     *           M(t)     1    |  m4  -m1  m1*m5-m2*m4 |
     *  M(-1) = ------ = --- * | -m3   m0  m2*m3-m0*m5 |
     *           |M|     |M|   |   0    0          |M| |
     *
     *  |M| = m0*m4 - m1*m3
     */
    static int
    invertAffineMatrix(double *d, const double *m)
    {
      double        dstM    = m[0]*m[4] - m[1]*m[3];
      if(dstM == 0.)
        return -1;

      d[0]  = m[4] / dstM;
      d[1]  = -m[1] / dstM;
      d[2]  = (m[1]*m[5] -  m[2]*m[4]) / dstM;
      d[3]  = -m[3] / dstM;
      d[4]  = m[0] / dstM;
      d[5]  = (m[2]*m[3] - m[0]*m[5]) /dstM;
      return 0;
    }

    static double *
    mulMatrix2D(double *d, const double *s0, const double *s1)
    {
      double m0, m1, m2, m3, m4, m5;
    #if 1
      m0    = s0[0] * s1[0] + s0[3] * s1[1];
      m1    = s0[1] * s1[0] + s0[4] * s1[1];
      m2    = s0[2] * s1[0] + s0[5] * s1[1] + s1[2];
      m3    = s0[0] * s1[3] + s0[3] * s1[4];
      m4    = s0[1] * s1[3] + s0[4] * s1[4];
      m5    = s0[2] * s1[3] + s0[5] * s1[4] + s1[5];
    #else
      m0    = s0[0] * s1[0] + s0[1] * s1[3];
      m1    = s0[0] * s1[1] + s0[1] * s1[4];
      m2    = s0[0] * s1[2] + s0[1] * s1[5] + s0[2];
      m3    = s0[3] * s1[0] + s0[4] * s1[3];
      m4    = s0[3] * s1[1] + s0[4] * s1[4];
      m5    = s0[3] * s1[2] + s0[4] * s1[5] + s0[5];
    #endif

      d[0]  = m0;
      d[1]  = m1;
      d[2]  = m2;
      d[3]  = m3;
      d[4]  = m4;
      d[5]  = m5;

      return d;
    }

    static double *
    buildIdentityMatrix2D()
    {
      double        *m      = (double *)malloc(sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      if(m == nullptr)
        return nullptr;

      memset(m, 0, sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      m[0]  = m[4]  = 1.0;
      return m;
    }

    /*
     *      |  1   0     0 |
     *  M = |  0  -1  2*ay |
     *      |  0   0     1 |
     */
    static double *
    buildMirrorVerMatrix2D(double ay)
    {
      double        *m      = (double *)malloc(sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      if(m == nullptr)
        return nullptr;

      m[0]  = 1.0;
      m[1]  = 0;
      m[2]  = 0;
      m[3]  = 0;
      m[4]  = -1.0;
      m[5]  = 2. * ay;

      return m;
    }

    /*
     *      | -1   0  2*ax |
     *  M = |  0   1     0 |
     *      |  0   0     1 |
     */
    static double *
    buildMirrorHorMatrix2D(double ax)
    {
      double        *m      = (double *)malloc(sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      if(m == nullptr)
        return nullptr;

      m[0]  = -1.0;
      m[1]  = 0;
      m[2]  = 2. * ax;
      m[3]  = 0;
      m[4]  = 1.0;
      m[5]  = 0;

      return m;
    }

    /*
     *      |  scale*cos(a)  scale*sin(a)  (1-scale*cos(a))*cx-scale*sin(a)*cy  |
     *  M = | -scale*sin(a)  scale*cos(a)  scale*sin(a)*cx+(1-scale*cos(a))*cy  |
     *      |             0             0                                    1  |
     */
    static double *
    buildRotationMatrix2D(const MtAffineFPoint *center, double angle, double scale)
    {
      double        *m      = (double *)malloc(sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      if(m == nullptr)
        return nullptr;
      double        pi      = acos(-1);
      double        rad     = angle * pi / 180.;
      double        cx      = center->x;
      double        cy      = center->y;

      m[0]  = scale*cos(rad);
      m[1]  = scale*sin(rad);
      m[2]  = (1 - scale*cos(rad))*cx - scale*sin(rad)*cy;
      m[3]  = -scale*sin(rad);
      m[4]  = scale*cos(rad);
      m[5]  = scale*sin(rad)*cx + (1 - scale*cos(rad))*cy;

      return m;
    }

    /*
     *      | scalex       0  -scalex*cx |
     *  M = |      0  scaley  -scaley*cy |
     *      |      0       0           1 |
     */
    static double *
    buildScaleMatrix2D(const MtAffineFPoint *center, double scalex, double scaley)
    {
      double        *m      = (double *)malloc(sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      if(m == nullptr)
        return nullptr;
      double        cx      = center->x;
      double        cy      = center->y;

      m[0]  = scalex;
      m[1]  = 0;
      m[2]  = -scalex*cx;
      m[3]  = 0;
      m[4]  = scaley;
      m[5]  = -scaley*cy;

      return m;
    }

    static void inline
    freeMatrix2D(double *m)
    {
      free(m);
    }

    /*
     *      | m0 m1 m2 |          |  a  c tx |
     *  s = | m3 m4 m5 | ->  d =  |  b  d ty |
     *      |  0  0  1 |          |  0  0  1 |
     */
    static inline void
    array2abcd(MtAffineMatrix *d, const double *s)
    {
      d->a  = s[0];
      d->b  = s[3];
      d->c  = s[1];
      d->d  = s[4];
      d->tx = s[2];
      d->ty = s[5];
    }

    /*
     *      |  a  c tx |          | m0 m1 m2 |
     *  s = |  b  d ty | ->  d =  | m3 m4 m5 |
     *      |  0  0  1 |          |  0  0  1 |
     */
    static inline void
    abcd2array(double *d, const MtAffineMatrix *s)
    {
      d[0]  = s->a;
      d[1]  = s->c;
      d[2]  = s->tx;
      d[3]  = s->b;
      d[4]  = s->d;
      d[5]  = s->ty;
    }

    static inline double *
    buildMatrix2DFromAbcd(const MtAffineMatrix *s)
    {
      double        *m      = (double *)malloc(sizeof(double) * MT_AFFINE_MATRIX_2D_SZ);
      abcd2array(m, s);
      return m;
    }

    int MtAffineCallType
    MtInvertAffineMatrix(MtAffineMatrix *d, const MtAffineMatrix *s)
    {
      double        *sa     = buildMatrix2DFromAbcd(s);
      double        *da     = buildIdentityMatrix2D();
      if(invertAffineMatrix(da, sa) < 0)
      {
        freeMatrix2D(sa);
        freeMatrix2D(da);
        return MT_AFFINE_RET_CODE_ERR;
      }
      array2abcd(d, da);
      freeMatrix2D(sa);
      freeMatrix2D(da);
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtMulMatrix2D(MtAffineMatrix *d, MtAffineMatrix *s0, const MtAffineMatrix *s1)
    {
      double        *sa0    = buildMatrix2DFromAbcd(s0);
      double        *sa1    = buildMatrix2DFromAbcd(s1);
      double        *da     = mulMatrix2D(sa0, sa0, sa1);
      array2abcd(d, da);
      freeMatrix2D(sa0);
      freeMatrix2D(sa1);
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtBuildIdentityMatrix2D(MtAffineMatrix *d)
    {
      d->a  = 1.;
      d->b  = 0.;
      d->c  = 0.;
      d->d  = 1.;
      d->tx = 0.;
      d->ty = 0.;
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtBuildOffsetMatrix2D(MtAffineMatrix *d, double tx, double ty)
    {
      d->a  = 1.;
      d->b  = 0.;
      d->c  = 0.;
      d->d  = 1.;
      d->tx = tx;
      d->ty = ty;
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtBuildMirrorVerMatrix2D(MtAffineMatrix *d, double ay)
    {
      double        *da     = buildMirrorVerMatrix2D(ay);
      array2abcd(d, da);
      freeMatrix2D(da);
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtBuildMirrorHorMatrix2D(MtAffineMatrix *d, double ax)
    {
      double        *da     = buildMirrorHorMatrix2D(ax);
      array2abcd(d, da);
      freeMatrix2D(da);
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtBuildRotationMatrix2D(MtAffineMatrix *d, const MtAffineFPoint *center,
                            double angle, double scale)
    {
      double        *da     = buildRotationMatrix2D(center, angle, scale);
      array2abcd(d, da);
      freeMatrix2D(da);
      return MT_AFFINE_RET_CODE_SUCC;
    }

    int MtAffineCallType
    MtBuildScaleMatrix2D(MtAffineMatrix *d, const MtAffineFPoint *center,
                         double scalex, double scaley)
    {
      double        *da     = buildScaleMatrix2D(center, scalex, scaley);
      array2abcd(d, da);
      freeMatrix2D(da);
      return MT_AFFINE_RET_CODE_SUCC;
    }

}
